package es.uji.icc.robot.ui;

import java.applet.Applet;
import java.awt.Button;
import java.awt.FlowLayout;
import java.awt.GridLayout;
import java.awt.HeadlessException;
import java.awt.Label;
import java.awt.Panel;
import java.awt.Scrollbar;
import java.awt.TextArea;
import java.awt.TextField;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.awt.event.AdjustmentEvent;
import java.awt.event.AdjustmentListener;
import java.text.SimpleDateFormat;
import java.util.Date;

import javax.swing.Timer;

import es.uji.icc.robot.fanuc.Joints;
import es.uji.icc.robot.fanuc.Robot;
import es.uji.icc.robot.fanuc.RobotException;
import es.uji.icc.robot.fanuc.RobotToolLocation;
import es.uji.icc.robot.grasp.Grasp;
import es.uji.icc.robot.grasp.GraspException;

@SuppressWarnings("serial")
public class Main extends Applet implements ActionListener, AdjustmentListener {

	
	Robot robot = null;	
	Grasp grasp = null;
	
	Panel controlPanel = new Panel();
	Panel scrollBarsPanel = new Panel();

	Panel scrollBarsPanelJoints = new Panel();
	Panel[] jointPanel = new Panel[6];
	Scrollbar[] jointScrollbar = new Scrollbar[6];
	TextField[] jointScrollbarTextValue = new TextField[6];
	TextField[] jointTextValue = new TextField[6];

	Panel scrollBarsPanelXYZ = new Panel();
	Panel[] XYZPanel = new Panel[6];
	Scrollbar[] XYZScrollbar = new Scrollbar[6];
	TextField[] XYZScrollbarTextValue = new TextField[6];
	TextField[] XYZTextValue = new TextField[6];
	
	Label labelURIRobotServer = new Label("URI (http://IP:port/URL) of the robot server: ");
	TextField textURIRobotServer = new TextField(40);		
	Label labelURIGraspServer = new Label("URI (http://IP:port/URL) of the grasp server: ");
	TextField textURIGraspServer = new TextField(40);
	TextArea textLog = new TextArea();

	Timer timer;

	Button connectRobotButton = new Button("Connect Robot");
	Button disconnectRobotButton = new Button("Disconnect Robot");
	Button isConnectedButton = new Button("Is connected?");
	Button pingRobotServerButton = new Button("Ping Robot Server");
	
	Button connectGraspButton = new Button("Connect Grasp");
	Button disconnectGraspButton = new Button("Disconnect Grasp");
	Button pingGraspServerButton = new Button("Ping Grasp Server");	
	
	Button openGraspButton = new Button("Open grasp");
	Button closeGraspButton = new Button("Close Grasp");

	public void init() {
		setSize(600, 600);
	}

	public Main() throws HeadlessException {

		this.setLayout(new GridLayout(2, 1));

		controlPanel.setLayout(new FlowLayout());

		controlPanel.add(pingRobotServerButton);
		controlPanel.add(connectRobotButton);
		controlPanel.add(isConnectedButton);
		controlPanel.add(disconnectRobotButton);
		controlPanel.add(labelURIRobotServer);
		controlPanel.add(textURIRobotServer);
		controlPanel.add(labelURIGraspServer);
		controlPanel.add(textURIGraspServer);
		
		controlPanel.add(pingGraspServerButton);
		controlPanel.add(openGraspButton);
		controlPanel.add(closeGraspButton);
		
		controlPanel.add(textLog);
		
		
		
		

		scrollBarsPanel.setLayout(new GridLayout(1, 2));
		scrollBarsPanelJoints.setLayout(new GridLayout(6, 1));
		scrollBarsPanelXYZ.setLayout(new GridLayout(6, 1));

		this.add(controlPanel);
		this.add(scrollBarsPanel);

		scrollBarsPanel.add(scrollBarsPanelJoints);
		scrollBarsPanel.add(scrollBarsPanelXYZ);

		for (int i = 0; i < 6; i++) {
			jointPanel[i] = new Panel();
			jointPanel[i].setLayout(new FlowLayout());
			jointScrollbar[i] = new Scrollbar(Scrollbar.HORIZONTAL, 0, 1, -360, 360);
			jointScrollbarTextValue[i] = new TextField(4);
			jointTextValue[i] = new TextField(4);
			jointScrollbarTextValue[i].setText("0");

			jointPanel[i].add(jointScrollbar[i]);
			jointPanel[i].add(jointScrollbarTextValue[i]);
			jointPanel[i].add(jointTextValue[i]);

			scrollBarsPanelJoints.add(jointPanel[i]);
		}
		for (int i = 0; i < 6; i++) {
			XYZPanel[i] = new Panel();
			XYZPanel[i].setLayout(new FlowLayout());
			XYZScrollbar[i] = new Scrollbar(Scrollbar.HORIZONTAL, 10, 10, -2000, 2000);
			XYZScrollbar[i].setUnitIncrement(10);
			XYZScrollbarTextValue[i] = new TextField(4);
			XYZTextValue[i] = new TextField(4);
			XYZScrollbarTextValue[i].setText("0");

			XYZPanel[i].add(XYZScrollbar[i]);
			XYZPanel[i].add(XYZScrollbarTextValue[i]);
			XYZPanel[i].add(XYZTextValue[i]);
			scrollBarsPanelXYZ.add(XYZPanel[i]);
		}

		connectRobotButton.addActionListener(this);
		disconnectRobotButton.addActionListener(this);
		pingRobotServerButton.addActionListener(this);
		isConnectedButton.addActionListener(this);
		
		connectGraspButton.addActionListener(this);
		disconnectGraspButton.addActionListener(this);		
		pingGraspServerButton.addActionListener(this);
			
		openGraspButton.addActionListener(this);
		closeGraspButton.addActionListener(this);
		
				
		textURIRobotServer.setText("http://localhost:4321/RestServiceImpl.svc");
		//textURIRobotServer.setText("http://150.128.87.210:9999/fanuc/RestServiceImpl.svc");		
		//textURIRobotServer.setText("http://150.128.87.212:9999/fanuc/RestServiceImpl.svc");
		//textURIGraspServer.setText("http://localhost:4322/RestServiceImpl.svc");		
		textURIGraspServer.setText("http://150.128.87.212:9998/grasp/RestServiceImpl.svc");
		

	}

	public void initializeValues() {

		
		printXYZPosition();
		for (int i = 0; i < 6; i++) {

			// Initialitzation of Scrollbar with the current robot position
			XYZScrollbar[i].setValue(Integer.parseInt(XYZTextValue[i].getText()));
			XYZScrollbarTextValue[i].setText(XYZTextValue[i].getText());
			// Only after initializing values, it is convenient to add the
			// listener to the Scrollbar
			XYZScrollbar[i].addAdjustmentListener(this);

		}
		printJointsValues();
		for (int i = 0; i < 6; i++) {
			// Initialitzation of Scrollbar with the current robot position
			jointScrollbar[i].setValue(Integer.parseInt(jointTextValue[i].getText()));
			jointScrollbarTextValue[i].setText(jointTextValue[i].getText());
			// Only after initializing values, it is convenient to add the
			// listener to the Scrollbar
			jointScrollbar[i].addAdjustmentListener(this);
		}
	}

	public void adjustmentValueChanged(AdjustmentEvent e) {

		if (e.getSource().equals(jointScrollbar[0]) || e.getSource().equals(jointScrollbar[1])
				|| e.getSource().equals(jointScrollbar[2])
				|| e.getSource().equals(jointScrollbar[3])
				|| e.getSource().equals(jointScrollbar[4])
				|| e.getSource().equals(jointScrollbar[5])) {

			for (int i = 0; i < 6; i++) {
				jointScrollbarTextValue[i].setText(String.valueOf(jointScrollbar[i].getValue()));
			}
			Joints j = new Joints(jointScrollbar[0].getValue(), jointScrollbar[1].getValue(),
					jointScrollbar[2].getValue(), jointScrollbar[3].getValue(),
					jointScrollbar[4].getValue(), jointScrollbar[5].getValue());
			try {
				robot.Move(j);
			} catch (RobotException re) {
				log(re.getMessage());
			}
		}
		if (e.getSource().equals(XYZScrollbar[0]) || e.getSource().equals(XYZScrollbar[1])
				|| e.getSource().equals(XYZScrollbar[2]) || e.getSource().equals(XYZScrollbar[3])
				|| e.getSource().equals(XYZScrollbar[4]) || e.getSource().equals(XYZScrollbar[5])) {

			for (int i = 0; i < 6; i++) {
				XYZScrollbarTextValue[i].setText(String.valueOf(XYZScrollbar[i].getValue()));
			}
			RobotToolLocation rtl = new RobotToolLocation(XYZScrollbar[0].getValue(),
					XYZScrollbar[1].getValue(), XYZScrollbar[2].getValue(),
					XYZScrollbar[3].getValue(), XYZScrollbar[4].getValue(),
					XYZScrollbar[5].getValue());
			try {
				robot.Move(rtl);
			} catch (RobotException re) {
				log(re.getMessage());
			}
		}

	}

	@Override
	public void actionPerformed(ActionEvent e) {
		if (robot == null) {
			robot = new Robot(textURIRobotServer.getText() ,3000);			
		}
		
		if (grasp == null) {
			grasp = new Grasp(textURIGraspServer.getText() ,3000);			
		}

		if (e.getSource().equals(connectRobotButton)) {
			try {
				log("Connecting robot...");
				robot.Connect();
				log("Robot connected!");
				initializeValues();
				timer = new Timer(1000, this);
				timer.setInitialDelay(2000);
				timer.start();
			} catch (RobotException re) {
				log(re.getMessage());
			}
		}			
		
		if (e.getSource().equals(openGraspButton)){
			try {
				grasp.Open();
			} catch (GraspException ge) {
				log(ge.getMessage());
			}			
		}
		
		if (e.getSource().equals(closeGraspButton)){
			try {
				grasp.Close();
			} catch (GraspException ge) {
				log(ge.getMessage());
			}			
		}
		
		if (e.getSource().equals(disconnectRobotButton)) {
			try {
				log("Disconnecting...");
				timer.stop();
				timer = null;
				robot.Disconnect();
				log("Disconnected!");
				initializeValues();
			} catch (RobotException re) {
				log(re.getMessage());
			} catch (java.lang.NullPointerException npe) {

			}
		}
		if (e.getSource().equals(isConnectedButton)) {
			try {
				if (robot.isConnected()) {
					log("Robot is connected");
				} else {
					log("Robot is not connected");
				}
			} catch (RobotException re) {
				log(re.getMessage());
			}
		}
		if (e.getSource().equals(pingRobotServerButton)) {
			try {
				log("Robot ping");
				log(robot.Ping());
			} catch (RobotException ex) {
				log("Robot ping Error: " + ex.getMessage());
			}
		}
		
		if (e.getSource().equals(pingGraspServerButton)) {
			try {
				log("Grasp ping");
				log(grasp.Ping());
			} catch (GraspException ge) {
				log("Grasp ping Error: " + ge.getMessage());
			}
		}
		
		if (e.getSource().equals(timer)) {
			printXYZPosition();
			printJointsValues();
		}

	}

	private void log(String s) {
		SimpleDateFormat formatter = new SimpleDateFormat("hh:mm:ss");
		Date date = new Date();
		String t = formatter.format(date);
		textLog.setText(t + ": " + s + "\n" + textLog.getText());
	}

	private void printJointsValues() {
		Joints j;
		try {
			j = robot.getJoints();
			jointTextValue[0].setText(String.valueOf(j.getJoint1()));
			jointTextValue[1].setText(String.valueOf(j.getJoint2()));
			jointTextValue[2].setText(String.valueOf(j.getJoint3()));
			jointTextValue[3].setText(String.valueOf(j.getJoint4()));
			jointTextValue[4].setText(String.valueOf(j.getJoint5()));
			jointTextValue[5].setText(String.valueOf(j.getJoint6()));
		} catch (RobotException e) {
			log("printJointsValues ERROR: " + e.getMessage());
		}
	}

	private void printXYZPosition() {

		RobotToolLocation rtl;
		try {
			rtl = robot.getXYZWPR();
			XYZTextValue[0].setText(String.valueOf(rtl.getLocation().getX()));
			XYZTextValue[1].setText(String.valueOf(rtl.getLocation().getY()));
			XYZTextValue[2].setText(String.valueOf(rtl.getLocation().getZ()));
			XYZTextValue[3].setText(String.valueOf(rtl.getOrientation().getX()));
			XYZTextValue[4].setText(String.valueOf(rtl.getOrientation().getY()));
			XYZTextValue[5].setText(String.valueOf(rtl.getOrientation().getZ()));
		} catch (RobotException e) {
			log("printXYZPosition ERROR: " + e.getMessage());
		}
	}

}
